/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package ticks.chromedome.subsystems;

import edu.wpi.first.wpilibj.Counter;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import ticks.chromedome.RobotMap;

/**
 *
 * @author Jared
 */
public class ShooterPID extends PIDSubsystem {

    private static final double Kp = 0.0;
    private static final double Ki = 1.1;
    private static final double Kd = 0.0;
    private SpeedController shooter_motor;
    private Counter shooter_optical;
    // Initialize your subsystem here
    public ShooterPID() {
        super("ShooterPID", Kp, Ki, Kd);
        shooter_motor = new Victor(RobotMap.SHOOTER_OPTICAL);
        shooter_optical = new Counter(RobotMap.SHOOTER_OPTICAL);
        

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
    }
    /**
     * Sets shooter setpoint AND DOES NOT ENABLE SHOOTER!!!!
     * @param shooterspeed 
     * the desired speed
     */
    public void setShooterSpeed(double shooterspeed){
        setSetpoint(shooterspeed);
}
    
    public void enableShooter(){
        enable();
    }
    public void disableShooter(){
        disable();
    }
    public boolean shooterAtSpeed(){
        return Math.abs(getPosition()-getSetpoint())<20;
    }
    
    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;
        return 1/(shooter_optical.getPeriod()*60);
    }
    
    protected void usePIDOutput(double output) {
        shooter_motor.set(output);
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
    }
}
